/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for Position interface client library.
 * \author Antti Maula <antti.maula@tkk.fi>
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 */

#include "PositionClient.hpp"
#include "PositionData.hpp"

//*****************************************************************************
using namespace MaCI::Position;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CPositionClient::CPositionClient(gimi::GIMI *aGIMIPtr, 
                                 const int aInterfaceMinor,
                                 const int aPositionBacklog_ms)
  : CSync(1,1),
    CThread(1),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_POSITION, aInterfaceMinor),
    iPositionData(),
    iPositionDataSequence(-1),
    iPositionClientCallback(NULL),
    iPositionBacklog(aPositionBacklog_ms, 10000),
    iCallBackUserPtr(NULL)
{
  dPrint(4,"CPositionClient %p constructed", this);
}
//*****************************************************************************

CPositionClient::~CPositionClient()
{
  
  DoClose();
  dPrint(4,"CPositionClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CPositionClient::DoOpen(void)
{
  MaCI::EMaCIError result = KMaCIOK;
  
  //Check for gimi pointer
  if(!IsThreadRunning()){
    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();
    
    if (result == KMaCIOK) {
      // All OK, so start the Thread
      
      SetActive(true);
      // Enable sync. message replies.
      EnableSynchronousMessageReply();
      CThread::RunThread();
    }
  }else{
    dPrint(1, "Position client already open!");
    result = KMaCIError;

  }
 
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CPositionClient::DoClose(void)
{
 
  MaCI::EMaCIError result = KMaCIError;

  if (IsThreadRunning()){
    SetActive(false);
    // Do GIMI magic
    // Wait for thread to complete.
    CThread::SetRequestTermination();
    UnsubscribeToInterface(TimeoutValue(2000));
    CThread::WaitThread();
  } else {
    dPrint(4, "PositionClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  return result;
}
//*****************************************************************************

bool CPositionClient::SetInitialize(void)
{
  // Create Pos container, and set it to use Internal BinBag
  CPositionData pos;
  pos.CreateInternalBinBag();

  // Assign new data (Call TTimestamp() with no parameters for current time)
  pos.SetTimestamp(Common::TTimestamp());
  pos.SetCommand(KCommandInitialize);

  // Send to GIMnet for default target.
  const EMaCIError e = SendInterfaceMessage(pos.GetBinBagContainerPtr()->GetBuffer(),
                                            pos.GetBinBagContainerPtr()->GetBufferSize());

  return (e == KMaCIOK);
}
//*****************************************************************************

bool CPositionClient::SetPosition(const TPose2D &aPos)
{
  // Create Pos container, and set it to use Internal BinBag
  CPositionData pos;
  pos.CreateInternalBinBag();

  // Assign new data (Call TTimestamp() with no parameters for current time)
  pos.SetTimestamp(Common::TTimestamp());
  pos.SetCommand(KCommandSetPose2D);
  pos.SetPose2D(aPos);

  // Send to GIMnet for default target.
  const EMaCIError e = SendInterfaceMessage(pos.GetBinBagContainerPtr()->GetBuffer(),
                                            pos.GetBinBagContainerPtr()->GetBufferSize());

  return (e == KMaCIOK);
}
//*****************************************************************************

bool CPositionClient::SetPosition(const TPose2D &aPos,const TVariance2D &aVar, const TProbability &aProb)
{
  // Create Pos container, and set it to use Internal BinBag
  CPositionData pos;
  pos.CreateInternalBinBag();

  // Assign new data (Call TTimestamp() with no parameters for current time)
  pos.SetTimestamp(Common::TTimestamp());
  pos.SetCommand(KCommandSetPose2D);
  pos.SetPose2D(aPos);
  pos.SetVariance2D(aVar);
  pos.SetProbability(aProb);

  // Send to GIMnet for default target.
  const EMaCIError e = SendInterfaceMessage(pos.GetBinBagContainerPtr()->GetBuffer(),
                                            pos.GetBinBagContainerPtr()->GetBufferSize());

  return (e == KMaCIOK);
}
//*****************************************************************************

bool CPositionClient::GetPositionEvent(CPositionData &aData, 
                                       int *aSeq, 
                                       const int aTimeout_ms)
{ 
  bool result = false;

  // Fallback in case the class is not active.
  if (!IsActive()) return false;

  Lock();
  
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    //dPrint(1,"Sequence %d",iPositionDataSequence);
    //  iPositionData.Print(1);
    aData = iPositionData;
    result = true;
    
  } else {
    
    if (iPositionDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();

      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KEventPositionData);
        time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iPositionDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iPositionDataSequence > *aSeq) {
        // Yes :)
        aData = iPositionData;
        *aSeq = iPositionDataSequence;
        //  dPrint(1,"Sequence %d",iPositionDataSequence);
        //iPositionData.Print(1);
        result = true;
        //dPrint(1,"Got data. *aSeq = %d, iPositionDataSequence = %d", *aSeq, iPositionDataSequence);
        
      } else {
        // No :(
        dPrint(3, "Failed to get PositionData! (Timeout %dms)",
               aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      //dPrint(1,"Sequence %d",iPositionDataSequence);
      //iPositionData.Print(1);
      aData = iPositionData;
      *aSeq = iPositionDataSequence;
      //      dPrint(1,"Got data. *aSeq = %d, iPositionDataSequence = %d", *aSeq, iPositionDataSequence);
      result = true;
    }
  }
  
  Unlock();
  
  return result;
}
//*****************************************************************************

bool CPositionClient::GetPositionEvent(CPositionData &aData, 
                                       const gim::time &aTime,
                                       gim::time *aTimeDiff)
{
  Lock();
  const bool result = iPositionBacklog.PeekData(aData, aTime, aTimeDiff);
  Unlock();

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CPositionClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      dPrint(8,"Got message, process it");
      ProcessMessage(gmsg);


    } else if (e != KMaCITimeout) {
      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
               e);
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error %u. Normal situtation because client is already closed",
               e);
      }
      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");

  return result;
}
//*****************************************************************************

int CPositionClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CPositionData positionData;
  bool r;
  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    
    positionData.Reset();
    r =  positionData.DecodeFrom(&bb);
    if (r) {
      
      //positionData.Print(20);

      // Get timestamp element
      const Common::TTimestamp *ts = positionData.GetTimestamp();
      gim::time pos_timestamp;
      if (ts != NULL) {
        // If the element is found, convert it to gim::time
        pos_timestamp.seconds = ts->timestamp_s;
        pos_timestamp.useconds = ts->timestamp_us;
      }
      
      // Now Lock()
      Lock();
      // Store data to 'most-current' container.
      iPositionData = positionData;
      ++iPositionDataSequence;
      // dPrint(1,"At processmessage..");
      // positionData.Print(1);
      // iPositionData.Print(1);
      // If timestamp was available, push to backlog
      if (ts) {
        //        dPrint(1,"Pushed Position to Backlog @ %s", pos_timestamp.toString().c_str());
        iPositionBacklog.PushData(pos_timestamp, positionData, false);
      }
      Unlock();
      Notify(KEventPositionData);

      // Call the handler if set.
      if (iPositionClientCallback) {
        dPrint(8,"Got Callback for PositionDataEvent - executing!");
        iPositionClientCallback->OnPositionDataEvent(positionData,
                                                     iPositionDataSequence,
                                                     *this,
                                                     iCallBackUserPtr);
        dPrint(8,"Callback for PositionDataEvent - execution complete!");
        
      }
     
      // Notify!
      //      dPrint(1,"New data received. iPositionDataSequence = %d", iPositionDataSequence);
    }
  }
  return result;
}
//*****************************************************************************

CPositionClientCallback *CPositionClient::GetPositionClientCallBack(void) {
  return iPositionClientCallback;
}

//*****************************************************************************
//*****************************************************************************
